#ifndef SERVO_VISUAL__VISUAL_CONTROL_HPP_
#define SERVO_VISUAL__VISUAL_CONTROL_HPP_

#include <memory>
#include <chrono>
#include <cmath>

#include <rclcpp/rclcpp.hpp>
#include <rclcpp/parameter_event_handler.hpp>
#include <rcl_interfaces/msg/set_parameters_result.hpp>

#include <tf2_ros/buffer.h>
#include <tf2_ros/transform_listener.h>

#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/twist_stamped.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <sensor_msgs/msg/joint_state.hpp>
#include <std_msgs/msg/float64.hpp>

#include <moveit_servo/servo_parameters.h>
#include <moveit_servo/servo.h>
#include <moveit/planning_scene_monitor/planning_scene_monitor.h>

#include <eigen3/Eigen/Dense>

namespace servo_visual {

class VisualControl : public rclcpp::Node {
public:
  VisualControl();
  void initialize();

  // 查询视觉控制是否结束
  bool isFinished() const;

private:
  // 回调
  void visual_callback(const geometry_msgs::msg::Point::SharedPtr msg);
  void force_callback(const geometry_msgs::msg::TwistStamped::SharedPtr msg);

  // 主循环
  void control_loop();

  // 控制逻辑
  void xAxisClosedLoopControl();
  void yAxisClosedLoopControl();
  void zAxisClosedLoopControl();
  void publishTwistCommand();
  void smoothData();
  void simulateCameraZ();

  // 初始化
  void setupPlanningSceneMonitor();
  void setupServo();

  // 参数回调
  rcl_interfaces::msg::SetParametersResult
  parameters_callback(const std::vector<rclcpp::Parameter> & parameters);

  // 通信
  rclcpp::Subscription<geometry_msgs::msg::Point>::SharedPtr visual_sub_;
  rclcpp::Subscription<geometry_msgs::msg::TwistStamped>::SharedPtr force_sub_;
  rclcpp::Publisher<std_msgs::msg::Float64>::SharedPtr tf_pub_;
  rclcpp::Publisher<geometry_msgs::msg::TwistStamped>::SharedPtr twist_cmd_pub_;
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr pose_pub_;
  rclcpp::TimerBase::SharedPtr timer_;

  // MoveIt/TF
  std::shared_ptr<planning_scene_monitor::PlanningSceneMonitor> planning_scene_monitor_;
  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;
  std::unique_ptr<moveit_servo::Servo> servo_;

  // 状态量
  geometry_msgs::msg::Point result_data_;
  Eigen::Vector3d current_visual_;
  Eigen::Vector3d current_position_;
  Eigen::Vector3d velocity_;
  Eigen::Vector3d position_reference_;
  Eigen::Vector3d current_force_;

  geometry_msgs::msg::Point last_data_;

  // 时序
  rclcpp::Clock::SharedPtr clock_;
  rclcpp::Time last_msg_time_;
  rclcpp::Time start_time_;
  double dt_{0.0};
  double simulation_phase_{0.0};
  double simulation_step_{0.0};

  // 参数
  double target_x_{0.0};
  double Kp_x_{0.0};
  double error_threshold_x_{0.0};
  double MAX_VEL_X_{0.0};

  double target_y_{0.0};
  double Kp_y_{0.0};
  double error_threshold_y_{0.0};
  double MAX_VEL_Y_{0.0};

  double target_z_{0.0};
  double Kp_z_{0.0};
  double error_threshold_z_{0.0};
  double MAX_VEL_Z_{0.0};

  // 参数更新句柄
  rclcpp::node_interfaces::OnSetParametersCallbackHandle::SharedPtr parameter_callback_handle_;

  // 运行标志
  bool control_active_{false};
  bool finished_{false};
};

// 对外工厂函数（与 .cpp 保持一致）
std::shared_ptr<VisualControl> create_and_initialize_visual_control();

}  // namespace servo_visual

#endif  // SERVO_VISUAL__VISUAL_CONTROL_HPP_
